/*** 
 * @_______________#########_______________________ 
 * @______________############_____________________ 
 * @______________#############____________________ 
 * @_____________##__###########___________________ 
 * @____________###__######_#####__________________ 
 * @____________###_#######___####_________________ 
 * @___________###__##########_####________________ 
 * @__________####__###########_####_______________ 
 * @________#####___###########__#####_____________ 
 * @_______######___###_########___#####___________ 
 * @_______#####___###___########___######_________ 
 * @______######___###__###########___######_______ 
 * @_____######___####_##############__######______ 
 * @____#######__#####################_#######_____ 
 * @____#######__##############################____ 
 * @___#######__######_#################_#######___ 
 * @___#######__######_######_#########___######___ 
 * @___#######____##__######___######_____######___ 
 * @___#######________######____#####_____#####____ 
 * @____######________#####_____#####_____####_____ 
 * @_____#####________####______#####_____###______ 
 * @______#####______;###________###______#________ 
 * @________##_______####________####______________ 
 * @
 * @Author: ipk518 121206530@qq.com
 * @Date: 2025-08-06 16:24:53
 * @LastEditors: ipk518 121206530@qq.com
 * @LastEditTime: 2025-09-21 11:04:55
 * @FilePath: /etws-sxk2307/src/Uio/fftw.cpp
 * @Description: 
 * @你把时间投在专业上，两三年，你就能在圈子里小有名气。四五年，你就能靠这个专业赚钱。过了10年以上，你就能成为这个领域的专家
 * @Copyright (c) 2025 by etws@quyujiang, All Rights Reserved. 
 */

#include "stdio.h"
#include "string.h"
#include "stdlib.h"
#include "stdbool.h"
#include <iostream>
#include <vector>
#include <complex>  // <-- 添加这一行
#include "math.h"
#include "fftw3.h"
#include "fftw.h"


#include <algorithm>
#include <cmath>
#include <numeric>
#include <limits>

using namespace std;

#define UP_SAMPLE_256   5
#define UP_SAMPLE_1024  20


// 1. 计算每个样本能量 e[n] = I[n]^2 + Q[n]^2
static void compute_energy(const uint64_t* iq, int len, std::vector<double>& energy) {
    energy.resize(len);
    double I = 0 ,Q = 0;
    for (int i = 0; i < len; ++i) {
        I = (int32_t)(iq[i] & 0xffffffff);
        Q = (int32_t)((iq[i]  >> 32) & 0xffffffff);
        energy[i] = I*I + Q*Q;
    }
}

// 信号有效段检测函数
static std::pair<size_t, size_t> detect_signal_segment(
    const uint64_t* iq_signal,
    int dataLen,
    double factor = 2.0,
    size_t minLen = 50,
    bool use_median = true) 
{   
    std::vector<std::pair<size_t, size_t>> segments;
    
    size_t n_samples = dataLen;
    if (n_samples == 0) {
        return {0, 0}; // 空信号
    }
    
    // 1. 计算每个样本能量 e[n] = I[n]^2 + Q[n]^2
    std::vector<double> energy;
    compute_energy(iq_signal,dataLen,energy);

    // 首先检查是否可能全部是有效信号
    double min_energy = *std::min_element(energy.begin(), energy.end());
    double max_energy = *std::max_element(energy.begin(), energy.end());

    // 如果能量范围很小，可能是全有效信号或全噪声
    double energy_ratio = max_energy / (min_energy + 1e-10); // 避免除零
    if(energy_ratio < 10.0) {
        return {0, dataLen}; // 全有效信号
    }
    
    // 2. 估计噪声底并设定阈值
    double noise_floor;
    if (use_median) {
        // 使用中位数估计噪声底
        std::vector<double> sorted_energy = energy;
        std::sort(sorted_energy.begin(), sorted_energy.end());
        noise_floor = sorted_energy[n_samples / 2]; // 中位数
    } else {
        // 使用平均值估计噪声底
        double sum = 0.0;
        for (const auto& e : energy) {
            sum += e;
        }
        noise_floor = sum / n_samples;

        noise_floor = noise_floor/factor;//平均信号的一半
    }
    
    double threshold = noise_floor * factor;
    // double threshold = noise_floor;
    
    // 3. 找到连续超过阈值的区间
    
    size_t start = 0;
    bool in_segment = false;
    
    for (size_t i = 0; i < n_samples; ++i) {
        if (energy[i] > threshold) {
            if (!in_segment) {
                start = i;
                in_segment = true;
            }
        } else {
            if (in_segment) {
                if (i - start >= minLen) {
                    segments.emplace_back(start, i - 1);
                }
                in_segment = false;
            }
        }
    }
    
    // 处理最后一个可能存在的段
    if (in_segment && n_samples - start >= minLen) {
        segments.emplace_back(start, n_samples - 1);
    }
    
    // 4. 选择合适区间
    if (segments.empty()) {
        return {0, 0}; // 未找到有效段
    }
    
    // 选择最长的区间
    size_t max_length = 0;
    size_t selected_idx = 0;
    
    for (size_t i = 0; i < segments.size(); ++i) {
        size_t length = segments[i].second - segments[i].first + 1;
        if (length > max_length) {
            max_length = length;
            selected_idx = i;
        }
    }
    
    return segments[selected_idx];
}


// 根据信号特性和性能要求选择FFT长度
static int adaptive_fft_length(int segment_length, int max_allowed_length = 4000) {

    #define FFTW_STANDARD 0
    
    // 定义优先级序列：性能好且常用的FFT长度
    const int preferred_lengths[2][7] = {{50, 100, 200, 500, 1000, 2000, 4000},
                                        {64, 128, 256, 512, 1024, 2048, 4096}};
    if (segment_length == 0) { //如果没有信号则返回0
        return 0;
    }
    // 首先尝试找到刚好容纳信号的最小长度
    for (int i = 0 ; i < 7; i++) {
        if (preferred_lengths[FFTW_STANDARD][i] > segment_length) {
            // 找到第一个大于等于信号长度的FFT长度
            if (i == 0 || preferred_lengths[FFTW_STANDARD][i] == segment_length) {
                return preferred_lengths[FFTW_STANDARD][i];
            } else {
                return preferred_lengths[FFTW_STANDARD][i-1];
            }
        }
    }
    
    // 如果信号太长，使用最大允许长度
    if (segment_length <= max_allowed_length) {
        return max_allowed_length;
    }
    
    // 如果信号长度超过最大允许长度，返回最大允许长度
    return max_allowed_length;
}


// 计算频率对应的FFT bin索引
static int frequency_to_bin(int fft_size = 128, double frequency = 100000, double sampling_rate = 5000000) {
    // 计算频率分辨率
    double freq_resolution = sampling_rate / fft_size;
    
    // 计算对应的bin索引
    int bin_index = static_cast<int>(std::round(frequency / freq_resolution));
    
    // 确保索引在有效范围内
    if (bin_index < 0) bin_index = 0;
    if (bin_index >= fft_size) bin_index = fft_size - 1;
    
    return bin_index;
}

// 每个 uint64_t 包含一个复数，高32位是实部，低32位是虚部
static std::vector<std::complex<double>> convert_uint64_to_complex(const uint64_t* data, size_t size) {
    std::vector<std::complex<double>> result;
    result.reserve(size);
    
    for (size_t i = 0; i < size; ++i) {
        // 提取实部和虚部（假设是32位有符号整数）
        int32_t real_part = static_cast<int32_t>(data[i] >> 32);
        int32_t imag_part = static_cast<int32_t>(data[i] & 0xFFFFFFFF);
        
        // 转换为双精度浮点数
        result.emplace_back(static_cast<double>(real_part), static_cast<double>(imag_part));
    }
    
    return result;
}


static std::pair<double,double> fftw_work(int N,uint64_t *Data,int index)
{
    double magnitude = 0.0,db = 0.0,phase = 0.0;

	int32_t I = 0, Q = 0;
    std::complex<double> m_fftData;
    std::pair<double,double> m_fftResult;

    fftw_complex *din0, *out0;
    fftw_plan p0;

    din0 = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * N);
    out0 = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * N);
    if ((din0 == NULL) || (out0 == NULL))
    {
        printf("Error:insufficient available memory\n");
    }
    else
    {
        for (int i = 0; i<N; i++)/*测试数据*/
        {
            I = (int32_t)(Data[i] & 0xffffffff);
            Q = (int32_t)((Data[i]  >> 32) & 0xffffffff);
            din0[i][0] = static_cast<double>(I);
            din0[i][1] = static_cast<double>(Q);
            out0[i][0] = 0;
            out0[i][1] = 0;
        }
    }
    //n:为数据个数，可以为任意正整数，但如果为一些小因子的乘积计算起来可以更有效，不过即使n为素数算法仍然能够达到O(nlogn)的复杂度。
    // *in	如果in和out指针相同为原位运算，否则为非原位运算。
    //sign::可以为正变换FFTW_FORWARD(-1)，也可以为逆变换FFTW_BACKWORD(+1)，实际上就是变换公式中指数项的符号。
    //需注意FFTW的逆变换没有除以N，即数据正变换再反变换后是原始数据的N倍。
    //flags::参数一般情况下为FFTW_MEASURE 或 FFTW_ESTIMATE。FFTW_MEASURE表示FFTW会先计算一些FFT并测量所用的时间，
    //以便为大小为n的变换寻找最优的计算方法。依据 机器配置和变换的大小（n），这个过程耗费约数秒（时钟clock精度）。
    //FFTW_ESTIMATE则相反，它直接构造一个合理的但可能是次最优的方 案。总体来说，如果你的程序需要进行大量相同大小的FFT，
    //并且初始化时间不重要，可以使用FFTW_MEASURE，否则应使用 FFTW_ESTIMATE。FFTW_MEASURE模式下in和out数组中的值会被覆盖，所以该方式应该在用户初始化输入数据in之前完成。
    p0 = fftw_plan_dft_1d(N, din0, out0, FFTW_FORWARD, FFTW_ESTIMATE);
    
    fftw_execute(p0); /* repeat as needed */
    fftw_destroy_plan(p0);

    // 分析结果：计算幅度和相位
    // std::vector<std::complex<double>> fftData(N);
	// std::vector<std::pair<double,double>> magnitudes(N);
    // for (int i = 0; i < N; i++) {

    //     fftData[i] = std::complex<double>(out0[i][0], out0[i][1]);

    //     // magnitudes[i] = np.abs(fft_result[i]) / N * 2
    //     // magnitudes_db[i] = 20 * math.log10(magnitudes[i] + 1e-10)  # 避免log(0)
    //     // fft_phase[i] = np.angle(fft_result[i], deg=True) % 360
    //     // 计算幅度
    //     magnitude = std::abs(fftData[i])/N * 2;
    //     db = 20 * log10(magnitude);

    //     // 计算相位（弧度）
    //     phase = std::arg(fftData[i]);
    //     // 归一化到[0, 2π]范围
    //     phase = std::fmod(phase + 2 * M_PI, 2 * M_PI);

    //     magnitudes[i] = std::make_pair(db, phase);// 换DB

    //     // printf("第%d组,IQ计算:幅度%f,相位%f ,-------------------, FFTW:幅度%f,相位%f.\n", i,magnitude,phase,magnitudes[i].first,magnitudes[i].second);
    // }
    // magnitudes.clear();
	// fftData.clear();

    //指定数据计算
    // 指定数据计算
    if (out0 != nullptr) {
        
        m_fftData = std::complex<double>(out0[index][0], out0[index][1]);
        magnitude = std::abs(m_fftData)/N * 2;
        db = 20 * log10(magnitude);
        // 使用 atan2 计算相位（弧度）
        phase = atan2(m_fftData.imag(), m_fftData.real());

        // 转换为度数
        double phase_degrees = phase * 180.0 / M_PI;

        // 归一化到 [0, 360] 范围
        if (phase_degrees < 0) {
            phase_degrees += 360.0;
        }
        // phase = std::arg(m_fftData);
        // phase = std::fmod(phase + 2 * M_PI, 2 * M_PI);
        m_fftResult = std::make_pair(db, phase_degrees);
    } else {
        // 处理异常情况，例如返回默认值或错误标识
        m_fftResult = std::make_pair(0.0, 0.0);  // 或根据业务需求定义错误返回值
    }

    if (din0 != nullptr) fftw_free(din0);
    if (out0 != nullptr) fftw_free(out0);
    din0 = nullptr;
    out0 = nullptr;

	
	return m_fftResult;
}


std::pair<double,double> fftTask(int len,uint64_t *Data)
{
    //1、找到有效信号段
    int m_size = len - 10;
    uint64_t *m_Data = &Data[10];
    
    
    
    auto segment = detect_signal_segment(m_Data,m_size);
    int segment_len = segment.second - segment.first + 1;//计算长度

    if(Data[30] == 0 || segment_len < 100) {
        int I_date = 0,Q_date = 0;
        double db = 0.0, phase = 0.0;
        I_date = static_cast<int>(Data[30] & 0xffffffff);  
        Q_date = static_cast<int>((Data[30] >> 32) & 0xffffffff); 

        db = (double)sqrt(pow(Q_date, 2) + pow(I_date, 2));
        db = 20 * log10(abs(db)); // 换DB
        
        phase = atan2(Q_date, I_date) * 180.0 / M_PI; // 转换为度  
        phase = fmod(phase + 360.0, 360.0); // 使相位保持在 [0, 360) 范围内
        std::cout << "使用第30个点计算: [" << db << ", " << phase << "]" << " , src=" << Data[30] << std::endl;
        return std::make_pair(db, phase);// 换DB
    } else {
        //2、长度换算
        int fft_len = adaptive_fft_length(segment_len);

        //3、 计算频率对应的FFT bin索引
        int bin_index = frequency_to_bin(fft_len);

        // std::cout << "有效数据: [" << m_Data[segment.first] << " , " << m_Data[segment.first+1] << "]" << std::endl;
        // std::cout << "检测到的有效段: [" << segment.first + 10 << ", " << segment.second + 10  << "]" << std::endl;
        // std::cout << "选择的FFT长度: " << fft_len << std::endl;
        // std::cout << "频率100000Hz对应的FFT bin索引: " << bin_index << std::endl;
        
        
        return fftw_work(fft_len,&m_Data[segment.first],bin_index);
    }
    
}